using System;
using System.Collections.Generic;
using System.Text;

namespace ElteIk.RoboSoccer.Core.Physics
{
	/// <summary>
	/// A simple class for 2D vector operations.
	/// </summary>
    public sealed class Vector2D : ElteIk.RoboSoccer.Core.Physics.IVector2D
	{
		#region Fields

		private float x;
		private float y;

		#endregion

		#region Lifecycle

		public Vector2D(float x, float y)
		{
			this.x = x;
			this.y = y;
		}

		public Vector2D(Vector2D vector)
			: this(vector.x, vector.y)
		{
		}

		public Vector2D()
			: this(0.0f, 0.0f)
		{
		}

		#endregion

		#region Properties

		public float X
		{
			get { return x; }
			set { x = value; }
		}

		public float Y
		{
			get { return y; }
			set { y = value; }
		}

		public bool IsNullVector
		{
			get
			{
				return x == 0 && y == 0;
			}
		}

		public float Length
		{
			get { return (float) Math.Sqrt((double)x * x + (double)y * y); }
		}

		public Vector2D Negated
		{
			get { return new Vector2D(-this.x, -this.y); }
		}

		public Vector2D Perpendicular
		{
			get { return new Vector2D(this.y, -this.x); }
		}

		public Vector2D Normalized
		{
			get
			{
				if (!IsNullVector)
				{
					return this / Length;
				}
				else return new Vector2D(this);
			}
		}

		#endregion

        #region Constant values (not the best place for them)

        public static readonly float DegreesToRadian = 180f / (float) Math.PI;

        #endregion

        #region Constant vectors

        public static Vector2D X_Axis
        {
            get { return new Vector2D(1.0f, 0.0f); }
        }

		public static Vector2D Y_Axis 
        {
            get { return new Vector2D(0.0f, 1.0f); }
        }

        public static Vector2D NullVector
        {
            get { return new Vector2D(); }
        }

		#endregion

		#region Binary operations

		public static Vector2D operator +(Vector2D a, Vector2D b)
		{
			return new Vector2D(a.x + b.x, a.y + b.y);
		}

		public static Vector2D operator -(Vector2D a, Vector2D b)
		{
			return new Vector2D(a.x - b.x, a.y - b.y);
		}

		public static Vector2D operator *(Vector2D a, float x)
		{
			return new Vector2D(a.x * x, a.y * x);
		}

		public static Vector2D operator /(Vector2D a, float x)
		{
			return new Vector2D(a.x / x, a.y / x);
		}

        public static float DotProduct(Vector2D a, Vector2D b)
        {
            return a.X * b.X + a.Y * b.Y;
        }

		public override bool Equals(object obj)
		{
			if (obj is Vector2D)
				return Equals(obj as Vector2D);

			return base.Equals(obj);
		}

		public bool Equals(Vector2D vector)
		{
			return this.x == vector.x && this.y == vector.y;
		}

		#endregion

		#region Conversions

		/// <summary>
		/// Converts a Vector2D to a FreeVector2D.
		/// </summary>
		/// <param name="vector"></param>
		/// <returns></returns>
		public static implicit operator FreeVector2D(Vector2D vector)
		{
			return new FreeVector2D(vector);
		}

		#endregion	

        #region Functions

        
        /// <summary>
        /// Copies the vector.
        /// </summary>
        /// <returns></returns>
        public Vector2D Clone()
        {
            return new Vector2D(this.x, this.y);
        }

        /// <summary>
        /// Gets the positive angle between the two vectors.
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <returns></returns>
        public static float GetAngle(Vector2D a, Vector2D b)
        {
            return (float) Math.Acos((double) Vector2D.DotProduct(a.Normalized, b.Normalized)) * Vector2D.DegreesToRadian;
        }

        /// <summary>
        /// Gets the angle by A should be rotated to become collinear with B.
        /// It returns degrees between -180 and 180.
        /// </summary>
        /// <param name="a"></param>
        /// <param name="b"></param>
        /// <returns></returns>
        public static float GetRotationAngle(Vector2D a, Vector2D b)
        {
            double angle_b = Math.Atan2(b.Y, b.X);
            double angle_a = Math.Atan2(a.Y, a.X);

            double angle = angle_b - angle_a;

            if (angle < -Math.PI)
                angle += 2 * Math.PI;
            else if (angle > Math.PI)
                angle -= 2 * Math.PI;

            return (float) (angle) * DegreesToRadian;
        }

        /// <summary>
        /// Make a normalized vector with the direction of degree (-180,180).
        /// </summary>
        /// <param name="degree"></param>
        /// <returns></returns>
        public static Vector2D DegreesToNormalizedVector(float degree)
        {
            float y = -(float)Math.Sin(degree / DegreesToRadian);
            float x = (float)Math.Cos(degree / DegreesToRadian);
            return new Vector2D(x, y).Normalized;
        }
        #endregion
    }
}
